#include "../include/armor_sim/ArmorSim.hpp"
#include <random>
using namespace std::chrono_literals;

namespace wmj
{
    ArmorSim::ArmorSim(std::string name) : Node(name)
    {

        armors_pub = this->create_publisher<base_interfaces::msg::Armors>("/Armors_base",1);
        timer_ptr_ = this->create_wall_timer(10ms, std::bind(&ArmorSim::armor_callback, this));

        init();
    }

    void ArmorSim::init()
    {
        static std::random_device rd;         
        static std::mt19937 gen(rd());        
        static std::uniform_real_distribution<double> dist_angle(0.0, 2.0 * PI);
        static std::uniform_real_distribution<double> dist_x(1.5, 5);
        static std::uniform_real_distribution<double> dist_y(-0.1, 0.1);
        static std::uniform_real_distribution<double> dist_z(-1, 7.5);

        angle = dist_angle(gen);
        center_x = dist_x(gen);
        center_y = dist_y(gen);
        center_z = dist_z(gen);

        if (center_y > 0)
            palstance = -0.8 * PI;
    }

    void ArmorSim::Outpost()
    {
        angle += palstance * 0.01;
        if (angle > 2 * PI)  angle -= 2 * PI;

    }

    void ArmorSim::armor_callback()
    {
        base_interfaces::msg::Armor armor_msg;
        base_interfaces::msg::Armors msg;

        
        Outpost();
        armor_msg.id = 11;
        armor_msg.armor_type = ARMOR_SMALL;

        rclcpp::Time current_time = this->now();

        armor_msg.header.stamp.sec = current_time.seconds();
        armor_msg.header.stamp.nanosec = current_time.nanoseconds() % 1000000000;


        if (PI * 2 / 3 < angle && angle < PI * 4 / 3)
        {
            armor_msg.armor_pose.position.x = center_x + radius * cos(angle);
            armor_msg.armor_pose.position.y = center_y + radius * sin(angle);
            armor_msg.armor_pose.position.z = center_z;
            armor_msg.yaw_angle = angle;
        }else if (PI * 4 / 3 < angle && angle  < PI * 2)
        {
            armor_msg.armor_pose.position.x = center_x + radius * cos(angle + PI * 2 / 3);
            armor_msg.armor_pose.position.y = center_y + radius * sin(angle + PI * 2 / 3);
            armor_msg.armor_pose.position.z = center_z + 0.1;
            armor_msg.yaw_angle = angle - PI * 2 / 3;
        }else{
            armor_msg.armor_pose.position.x = center_x + radius * cos(angle + PI * 4 / 3);
            armor_msg.armor_pose.position.y = center_y + radius * sin(angle + PI * 4 / 3);
            armor_msg.armor_pose.position.z = center_z + 0.2;
            armor_msg.yaw_angle = angle + PI * 2 / 3;
        }

        msg.num = 1;
        msg.header.frame_id = "left_camera_frame";
        msg.armors.push_back(armor_msg);
        
        armors_pub->publish(msg);
    }
}

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto armorSim = std::make_shared<wmj::ArmorSim>("armorSim");
    rclcpp::spin(armorSim);
    rclcpp::shutdown();
    return 0;
}